#include <TimerOne.h>
//#include <String.h>
int ENABLE_DONGCO = 5;
int IN1 = 7;
int IN2 = 8;
int IN3 = 9;
int IN4 = 10;
char control;
//String control;
void setup() {  
  Serial.begin(9600);
  while (!Serial) {
  }
  pinMode(ENABLE_DONGCO, OUTPUT);
  pinMode(IN1, OUTPUT);  
  pinMode(IN2, OUTPUT);   
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  Serial.print("System is ready!");
  Timer1.initialize(100000); //set a timer of length 100000 microseconds (or 0.1 sec - or 10Hz => the led will blink 5 times, 5 cycles of on-and-off, per second)
  Timer1.attachInterrupt( timerIsr );
}

// the loop routine runs over and over again forever:
void loop() {
  int dodai;
  while(true){
    //Dieu khien dong co
    control=Serial.read();
    //dodai= strlen(control);
    delay(100);
    if( control=='w') ditoithang();
    if( control=='s') diluithang();
    if( control=='q') ditoiretrai();
    if( control=='e') ditoirephai();
    if( control=='z') diluiretrai();
    if( control=='c') diluirephai();
    if (control=='p') dungyen();
  }
}
//hàm điều hướng
void navigation(char control)
{
    if( control=='w') ditoithang();
    if( control=='s') diluithang();
    if( control=='q') ditoiretrai();
    if( control=='e') ditoirephai();
    if( control=='z') diluiretrai();
    if( control=='c') diluirephai();
    if (control=='p') dungyen();
}
//
//hàm điều khiển tốc độ
void speed(char control[3])
{
  char navigator, speeder;
  navigator = control[1];
  speeder = control[2];
}
void timerIsr()
{
    analogWrite(ENABLE_DONGCO, 190);
}
//============================================
void thang()
{
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,LOW);
}
void trai()
{
   digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
}
void phai()
{
   digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
}
//============================================
void dungyen()
{
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,LOW);
  thang();
}
void ditoi()
{
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  
}
void dilui()
{
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
}
//===========================
void ditoithang()
{
   ditoi();
  thang();
  
}
void diluithang()
{
   dilui();
  thang();
}
void ditoirephai()
{
  phai();
  ditoi();
}
void ditoiretrai()
{
  trai();
  ditoi();
}
void diluirephai()
{
   phai();
  dilui();
}
void diluiretrai()
{
   trai();
  dilui();
}
//WRITEN BY FAP FAP FAP_09T2_ITF_DUT
//điều khiển tốc độ
/*
lấy hai kí tự sau của chuỗi lệnh
chuyển thành số lưu vào potValue
motorValue = map(potValue, 0, 100, 0, 255);
analogWrite(ENABLE_DONGCO, motorValue); 
 */

